import os
import numpy as np
import math
from utils import *

def count_rad(rad):
    if rad > math.pi:
        rad -= math.pi * 2
        return count_rad(rad)
    elif rad < - math.pi:
        rad += math.pi *2
        return count_rad(rad)
    else:
        return rad

#
class Agent:
    def __init__(self, cfg) -> None:
        self.Eta_att = cfg.Eta_att          # 引力的增益系数
        self.Eta_rep_ob = cfg.Eta_rep_ob    # 斥力的增益系数
        # self.Eta_rep_edge = cfg.Eta_rep_edge# 道路边界斥力的增益系数
        self.d0 = cfg.d0                    # 障碍影响的最大距离
        self.len_step = cfg.len_step        # 步长
        self.n = cfg.n
        self.memory_box = cfg.memory_box
        self.vel_polish = cfg.vel_polish
        self.ang_polish = cfg.ang_polish
        self.vel_hist = []
        self.ang_hist = []
        
    def compute_action(self, state : list):
        # print(state)
        veh_id = state[0]
        vec2tar = state[1:3]   # list 实际上存的是方向向量 方向指向目标
        vel = state[3]
        att_rad = state[4]
        vec4obs = state[5]     # dict 实际上存的是方向向量 方向指向车辆
        obs_num = len(vec4obs)
        
        dists = {}
        unite_vec = {} #  保存载具当前位置与障碍物的单位方向向量，方向指向车辆；以及保存载具当前位置与目标点的单位方向向量，方向指向目标点
        F_rep_ob = {}  # 存储每一个障碍到车辆的斥力,带方向
        
        #计算车辆当前位置与障碍物的单位方向向量
        for key, val in vec4obs.items():
            dists[key] = np.linalg.norm(val)
            unite_vec[key]=val/dists[key]
            
        #计算车辆当前位置与目标的单位方向向量
        dists[0] = np.linalg.norm(vec2tar)
        unite_vec[0] = vec2tar/dists[0]
        
        # 计算目标点引力
        F_att = self.Eta_att*dists[0]*unite_vec[0]
        # print(F_att)
        # 计算障碍物斥力
        
        for key, val in vec4obs.items():
            if dists[key] > self.d0:
                F_rep_ob[key] = [0., 0.]
            else:
                # 障碍物的斥力1，方向由障碍物指向车辆
                F_rep_ob1_abs = self.Eta_rep_ob * (1 / dists[key] - 1 / self.d0) * (dists[0])**self.n / dists[key] ** 2  # 斥力大小
                F_rep_ob1 = F_rep_ob1_abs*unite_vec[key]  # 斥力向量
                # 障碍物的斥力2，方向由车辆指向目标点
                F_rep_ob2_abs = self.n/2 * self.Eta_rep_ob * (1 / dists[key] - 1 / self.d0) **2 *(dists[0])**(self.n-1) # 斥力大小
                F_rep_ob2 = F_rep_ob2_abs * unite_vec[0]  # 斥力向量
                # 改进后的障碍物合斥力计算
                F_rep_ob[key] = F_rep_ob1 + F_rep_ob2
        
        # 计算合力及合力方向
        F_sum = F_att + np.sum(np.array(list(F_rep_ob.values())), axis=0)
        UnitVec_Fsum = 1 / np.linalg.norm(F_sum) * F_sum    # 方向向量
        # print(F_sum)
        # print(UnitVec_Fsum)
        
        # 计算合力方向与速度方向夹角
        temp_ang = math.atan2(UnitVec_Fsum[1], UnitVec_Fsum[0]) - att_rad   # rad
        temp_ang = count_rad(temp_ang)
        
        temp_offset = 0.2
        if abs(temp_ang) >= math.radians(60.):
            temp_offset += 0.5 + 75. * abs(np.linalg.norm(F_sum) * math.cos(temp_ang))
        # temp_offset = 0.1
        
        temp_vel = min(max(np.linalg.norm(F_sum) * math.cos(temp_ang), -15.), 15.) - vel - temp_offset #+ 30.   # m/s
        
        dvel = 1 / 7.5 * temp_vel # if np.linalg.norm(vec2tar) > 10. else (0.1 * temp_vel)  
        
        dangvel = 1 / 30. * temp_ang if abs(temp_ang) >= math.radians(15.) else (0.5 * temp_ang) # math.pow(1 / 30., 2) *  * 2 # / abs(temp_ang) *math.pow(2 * abs(temp_ang), 1/2.) 
        # print(dangvel)
                
        if np.linalg.norm(vec2tar) <= 1.:
            if abs(temp_ang) >= math.radians(90.):
                dangvel = 0.5 * dangvel
        
        # 增加平滑处理（记忆合力）
        if len(self.ang_hist) > self.memory_box:
            self.vel_hist.pop(0)
            self.ang_hist.pop(0)
        
        # 均值修正
        # if abs(dvel) > abs(3 * np.mean(self.vel_hist)):
        #     dvel = self.vel_polish * dvel + ()
        
        
        
        if abs(temp_ang) <= math.radians(5.) :  # 设置大了会导致nan
        # if abs(dangvel) < abs(1 * np.mean(self.ang_hist)): or math.radians(90.) - abs(temp_ang) <= math.radians(5.)
            dangvel = 0.6 * dangvel + 0.4 * np.mean(self.ang_hist)
        
        self.vel_hist.append(dvel)
        self.ang_hist.append(dangvel)
        
        action = np.clip([dvel, dangvel], *[-1, 1]) # 加速度、角加速度 math.atan2(UnitVec_Fsum[0], UnitVec_Fsum[1])
        
        return action
    
    

if __name__ == '__main__':
    # 测试程序
    state_list = [3, 1., 0., 30, 0.78, {1: [-1817.4653119054005, -1089.6106271666215], 2: [667.1138801075059, -1537.6270627002318], 4: [1171.6921972218167, -364.745012791048], 5: [1014.5125087650385, -948.4758349247992]}, 0.0, False]
    # 读取初始化配置
    init_cfgs = process_yaml_cfg(path_to_configs='/root/code/workspace/src/control_policy/control_policy/MVS/Configs/', filename='InitConfigs.yaml')
    
    veh_params = init_cfgs.vehicle_init_msg['params']
    recv_ports_head = init_cfgs.decider_ports_cfg['recv_ports_head']
    send_port = init_cfgs.decider_ports_cfg['send_port']
    # 读取决策端配置
    decision_cfg = init_cfgs.decision_cfg
    algo_cfg = decision_cfg['algo_cfg']
    agent = Agent(algo_cfg)
    
    print(math.degrees(math.atan2(1,-1)))
    
    # while True:
    #     action = agent.compute_action(state_list)
    #     print(action)



